///////////////////////////////////////////////////
// Main file that controls the robot's movements //
///////////////////////////////////////////////////

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <signal.h>
#include <iostream>

#include "robot/robot.h"
#include "control/RobControl.h"
#include "udp/UDPReceiver.h"

Robot rob;

void stopRobot(int s);

int main () {
	int dest_x,dest_y;
	// add handler for CTRL+C
	signal(SIGINT, stopRobot);

	RobControl control;
	Info inf;
	int distance, theta;
	int sensor_dist;
    
	// wait for the first set of valid coordinates
	do { inf = receiveCoords(); }
	while ( !control.isValidCoords(inf) );
	
	// first destination is the can at (doelx, doely)
	dest_x=inf.doelx; dest_y=inf.doely;	

	rob.servoOpen();	// open servo at start
	
	for (int j=1;j<=2;j++) {	// 2 routes, 1 = going to can, 2 = going to garage
		
		// get initial distance to goal
		distance = (int)control.calc_distance(inf.robx, inf.roby,dest_x,dest_y);

		// while not in treshold, send drive commands
		while(distance > TRESH_STOP) {
			printf("\n");

			// wait for new coords
			do { inf = receiveCoords(); }
			while( !control.isValidCoords(inf) );
		
			// Update to current goal coords
			if(j==1){
				dest_x=inf.doelx; dest_y=inf.doely;
			}
			else {
				dest_x=inf.garx,dest_y=inf.gary;
			}
			
			// Calculate angle between robot and goal
			theta = (int)control.turn_calculate(inf.robx, inf.roby, inf.robhoek,dest_x,dest_y);

			// Rotate anti clockwise to goal to align with it
			if(theta>TURN_DEGREE) {
				if(theta>TRESH_THETA)
					rob.rotate(-MAXROTSPEED);
				else
					rob.rotate(-MINROTSPEED);
			}
			// Rotate clockwise to goal
			else if(theta<-TURN_DEGREE) {
				if(theta< -TRESH_THETA)
					rob.rotate(MAXROTSPEED);
				else
					rob.rotate(MINROTSPEED);
			}
			// Else we are aligned with goal, go to it
			else {
				if(distance>TRESH_DISTANCE)
					rob.setSpeed(MAXSPEED);
				else {
					rob.setSpeed(MINSPEED);
					//sensor_dist = rob.getDistance();
					//printf("\n\n\nSensor = %d\n\n\n", sensor_dist);
				}
			}
			
			// Update distance for next loop
			distance = (int)control.calc_distance(inf.robx, inf.roby,dest_x,dest_y);
		}
		
		// We reached the target, stop the robot		
		rob.stop();

		// If we reached the can, close the servo and update the destination to the garage
		if(j==1){
			rob.servoClose();
			printf("\nthe can is caught!\n");
			dest_x=inf.garx,dest_y=inf.gary;
		}
		// If we reached the garage, open servo to place the can in garage
		else {
			rob.servoOpen();
			printf("\nthe garage is reached!\n");
		}
	}

	// We reached the end, drive backwards for 2 seconds
	rob.stop();
	rob.setSpeed(-MAXSPEED);
	sleep(2);
	rob.stop();

	// Cleanup
	rob.destroy();   
}

// Ctrl+C signal handler
void stopRobot(int s) {
	rob.destroy();
	exit(0);
}
